微分动态规划

最新推荐文章于 2023-12-25 18:37:08 发布

微丶念

最新推荐文章于 2023-12-25 18:37:08 发布

阅读量6.3k

点赞数 2

本文链接：https://blog.csdn.net/u013745804/article/details/60878093

版权

from：https://en.wikipedia.org/wiki/Differential_dynamic_programming

深入理解DDP

DDP是一种轨迹优化类别问题中的最优控制算法。这种算法在1966年被Mayne提出。

该算法使用动态模型(dynamics)以及代价函数(cost functions)的局部二次(locally-quadratic)模型，并且展现二次收敛(displays quadratic convergence)性质。它与Pantoja's step-wise Newton's method有很大关联。

Finite-horizon discrete-time problems

下面我们来看看所要研究的问题：

The dynamics:

$\mathbf {x} _{i+1}=\mathbf {f} (\mathbf {x} _{i},\mathbf {u} _{i})$

从状态x出发，使用控制序列 $\mathbf {U} \equiv \{\mathbf {u} _{0},\mathbf {u} _{1}\dots ,\mathbf {u} _{N-1}\}$ 直到horizon is reached。

$J_{0}(\mathbf {x} ,\mathbf {U} )=\sum _{i=0}^{N-1}\ell (\mathbf {x} _{i},\mathbf {u} _{i})+\ell _{f}(\mathbf {x} _{N}),$

其中 $\mathbf {x} _{0}\equiv \mathbf {x}$

\mathbf {U} ^{*}(\mathbf {x} )\equiv \operatorname {argmin} _{\mathbf {U} }J_{0}(\mathbf {x} ,\mathbf {U} ).

轨迹优化(Trajectory optimization)意味着对于某一个 $\mathbf {x} _{0}$ 找到一个 $\mathbf {U} ^{*}(\mathbf {x} )$ 使得代价函数最小，而不是对于所有可能的初始状态(rather than for all possible initial states)。

Dynamic programming

设 $\mathbf {U} _{i}$ 是控制序列中的一部分 $\mathbf {U} _{i}\equiv \{\mathbf {u} _{i},\mathbf {u} _{i+1}\dots ,\mathbf {u} _{N-1}\}$ ，并且定义 cost-to-go $J_{i}$ 作为从

$i$ $N$

$J_{i}(\mathbf {x} ,\mathbf {U} _{i})=\sum _{j=i}^{N-1}\ell (\mathbf {x} _{j},\mathbf {u} _{j})+\ell _{f}(\mathbf {x} _{N}).$

其中令 $V(\mathbf {x} ,N)\equiv \ell _{f}(\mathbf {x} _{N})$

$V(\mathbf {x} ,i)=\min _{\mathbf {u} }[\ell (\mathbf {x} ,\mathbf {u} )+V(\mathbf {f} (\mathbf {x} ,\mathbf {u} ),i+1)].$

这就是Bellman equation。

Differential dynamic programming

DDP是如何运行的呢？

它通过迭代运行backward pass和forward pass来进行规划求解的。

DDP proceeds by iteratively performing a backward pass on the nominal trajectory to generate a new control sequence, and then a forward pass to compute and evalute a new nominal trajectory.

首先，我们看看backward pass是一个什么样的东西。

在上面一节的Bellman方程中，需要最小化的项为：

$\ell (\mathbf {x} ,\mathbf {u} )+V(\mathbf {f} (\mathbf {x} ,\mathbf {u} ),i+1)$

设 $Q$ 为该量在 $i$ -th $(\mathbf {x} ,\mathbf {u} )$

${\begin{aligned}Q(\delta \mathbf {x} ,\delta \mathbf {u} )\equiv &\ell (\mathbf {x} +\delta \mathbf {x} ,\mathbf {u} +\delta \mathbf {u} )&&{}+V(\mathbf {f} (\mathbf {x} +\delta \mathbf {x} ,\mathbf {u} +\delta \mathbf {u} ),i+1)\\-&\ell (\mathbf {x} ,\mathbf {u} )&&{}-V(\mathbf {f} (\mathbf {x} ,\mathbf {u} ),i+1)\end{aligned}}$

（我们知道，变分为0时也就达到了极值）。

将该式展开为二阶形式（Taylor展开即可，比如先按delta_x展开，再按delta_u展开）：

$\approx {\frac {1}{2}}{\begin{bmatrix}1\\\delta \mathbf {x} \\\delta \mathbf {u} \end{bmatrix}}^{\mathsf {T}}{\begin{bmatrix}0&Q_{\mathbf {x} }^{\mathsf {T}}&Q_{\mathbf {u} }^{\mathsf {T}}\\Q_{\mathbf {x} }&Q_{\mathbf {x} \mathbf {x} }&Q_{\mathbf {x} \mathbf {u} }\\Q_{\mathbf {u} }&Q_{\mathbf {u} \mathbf {x} }&Q_{\mathbf {u} \mathbf {u} }\end{bmatrix}}{\begin{bmatrix}1\\\delta \mathbf {x} \\\delta \mathbf {u} \end{bmatrix}}$

为了方便，我们将i去掉，并且利用prime(单引号)表示下一个时间步 $V'\equiv V(i+1)$ ，该展开的系数分别为：

${\begin{alignedat}{2}Q_{\mathbf {x} }&=\ell _{\mathbf {x} }+\mathbf {f} _{\mathbf {x} }^{\mathsf {T}}V'_{\mathbf {x} }\\Q_{\mathbf {u} }&=\ell _{\mathbf {u} }+\mathbf {f} _{\mathbf {u} }^{\mathsf {T}}V'_{\mathbf {x} }\\Q_{\mathbf {x} \mathbf {x} }&=\ell _{\mathbf {x} \mathbf {x} }+\mathbf {f} _{\mathbf {x} }^{\mathsf {T}}V'_{\mathbf {x} \mathbf {x} }\mathbf {f} _{\mathbf {x} }+V_{\mathbf {x} }'\cdot \mathbf {f} _{\mathbf {x} \mathbf {x} }\\Q_{\mathbf {u} \mathbf {u} }&=\ell _{\mathbf {u} \mathbf {u} }+\mathbf {f} _{\mathbf {u} }^{\mathsf {T}}V'_{\mathbf {x} \mathbf {x} }\mathbf {f} _{\mathbf {u} }+{V'_{\mathbf {x} }}\cdot \mathbf {f} _{\mathbf {u} \mathbf {u} }\\Q_{\mathbf {u} \mathbf {x} }&=\ell _{\mathbf {u} \mathbf {x} }+\mathbf {f} _{\mathbf {u} }^{\mathsf {T}}V'_{\mathbf {x} \mathbf {x} }\mathbf {f} _{\mathbf {x} }+{V'_{\mathbf {x} }}\cdot \mathbf {f} _{\mathbf {u} \mathbf {x} }.\end{alignedat}}$

关于 $\delta\mathbf{u}$ 最小化上面的二阶展开式，有

${\delta \mathbf {u} }^{*}=\operatorname {argmin} \limits _{\delta \mathbf {u} }Q(\delta \mathbf {x} ,\delta \mathbf {u} )=-Q_{\mathbf {u} \mathbf {u} }^{-1}(Q_{\mathbf {u} }+Q_{\mathbf {u} \mathbf {x} }\delta \mathbf {x} ),$

上面是通过线性二次最优问题的解得出，这里为什么要最小化变分呢？首先，上面的变分作为一个二次项形式，总是大于等于0的，我们并不去直接求解变分为0，而是最小化变分，这样的话假使变分不能为0，也即没有0解，同样也可以得到一个 $\delta\mathbf{u}$ 使得变分尽可能小，也就是代价函数的变化尽可能小，也即趋于收敛。

定义一个open-loop term $\mathbf {k} =-Q_{\mathbf {u} \mathbf {u} }^{-1}Q_{\mathbf {u} }$ ，以及一个feedback gain term $\mathbf {K} =-Q_{\mathbf {u} \mathbf {u} }^{-1}Q_{\mathbf {u} \mathbf {x} }$ ，将上面的最优 $\delta\mathbf{u}$

${\begin{alignedat}{2}\Delta V(i)&=&{}-{\tfrac {1}{2}}Q_{\mathbf {u} }Q_{\mathbf {u} \mathbf {u} }^{-1}Q_{\mathbf {u} }\\V_{\mathbf {x} }(i)&=Q_{\mathbf {x} }&{}-Q_{\mathbf {u} }Q_{\mathbf {u} \mathbf {u} }^{-1}Q_{\mathbf {u} \mathbf {x} }\\V_{\mathbf {x} \mathbf {x} }(i)&=Q_{\mathbf {x} \mathbf {x} }&{}-Q_{\mathbf {x} \mathbf {u} }Q_{\mathbf {u} \mathbf {u} }^{-1}Q_{\mathbf {u} \mathbf {x} }.\end{alignedat}}$

实际上，这里的Delta_V是1的系数，Vx是delta_x的系数，Vxx是(delta_x)^2的系数。

然后，我们反复递归计算 $V(i)$ 的局部二次模型(local quadratic models)以及control modifications $\{\mathbf {k} (i),\mathbf {K} (i)\}$ ，from $i=N-1$ down to $i=1$

$上面这些也就组成了backward pass。$

其中 $V(\mathbf {x} ,N)\equiv \ell _{f}(\mathbf {x} _{N})$

一旦上面的backward pass完成之后，我们就可以通过forward pass来计算一个新的轨迹(trajectory)：

${\begin{aligned}{\hat {\mathbf {x} }}(1)&=\mathbf {x} (1)\\{\hat {\mathbf {u} }}(i)&=\mathbf {u} (i)+\mathbf {k} (i)+\mathbf {K} (i)({\hat {\mathbf {x} }}(i)-\mathbf {x} (i))\\{\hat {\mathbf {x} }}(i+1)&=\mathbf {f} ({\hat {\mathbf {x} }}(i),{\hat {\mathbf {u} }}(i))\end{aligned}}$